/*倒立摆控制与仿真*/
#include "pendulum.hpp"
#include "controller.hpp"
#include "zhnoptim.hpp"

class CostFcn: public zhnoptim::UserFunc {
public:
    CostFcn() {
        mpidinner = new PID_Controller(&sim1, 10, 0, 100, 100);
        mpidouter = new PID_Controller(&sim1, 0.023186584, -0.0086849721, -63.246455, 709.7928);
        mpendulum = new InversePendulum(&sim1);
        SMConstant(minu, &sim1);
        SMSum(msuminner, &sim1);
        SMSum(msumouter, &sim1);
        SMOutput(mouttheta, &sim1);
        SMOutput(moutx, &sim1);
        sim1.connect(minu, msumouter);
        sim1.connect(msumouter, mpidouter);
        sim1.connect(mpidouter, msuminner);
        sim1.connect(msuminner, mpidinner);
        sim1.connect(mpidinner, mpendulum);
        sim1.connect(mpendulum, 0, msumouter);
        sim1.connect(mpendulum, 2, msuminner);
        msumouter->Set_InputGain(-1);
        msuminner->Set_InputGain(-1);
        sim1.connect(mpendulum, 0, moutx);
        sim1.connect(mpendulum, 2, mouttheta);
        sim1.Set_SampleTime(0.1);
        sim1.Set_Duration(10);
        sim1.Set_DivergenceCheckMode(4);
        sim1.Initialize();
        _simstep = sim1.Get_SimStep();
    };
    virtual double Function(const std::vector<double>& u) {
        double cost = 0;
        sim1.Simulation_Reset();
        mpidouter->Set_Param(u);
        if (sim1.Simulate())
            return 1e16;
        vecdble datas = moutx->Get_StoredData();
        for (double data: datas)
            cost += data*data;
        datas = mouttheta->Get_StoredData();
        for (double data: datas)
            cost += data*data;
        return cost;
    };
private:
    Simulator sim1;
    PID_Controller *mpidinner=nullptr;  // 内环PID控制器
    PID_Controller *mpidouter=nullptr;  // 外环PID控制器
    InversePendulum *mpendulum=nullptr;  // 倒立摆系统
    // MInput *minu=nullptr;  // 输入期望角度
    MConstant *minu=nullptr;  // 输入期望角度
    MSum *msuminner=nullptr;  // 角度误差
    MSum *msumouter=nullptr;  // 位置误差
    MOutput *mouttheta=nullptr;  // 摆杆角度
    MOutput *moutx=nullptr;  // 小车位移
    double _simstep;
};

int main()
{
    zhnoptim::Differential_Evolution de1(vecdble{0.64056276, 2.6843531, 0.80895114, 0.75105566});
    CostFcn *f = new CostFcn;
    de1.Set_CostFunction(f);
    de1.run();
    return 0;
}
